/*
 * mavlink.c
 * Michael du Breuil
 */

#include "common.h"
#include "mav_process.h" //THIS IS THE LOCAL HEADER, this may be a bad naming conveetion
#include <unistd.h>
#define __USE_BSD
#include <fcntl.h>
#define __USE_MISC 1 //needed for termios.h to provide CRTCTS
#include <termios.h>

static int fd;

int mav_init(void) {
  struct termios options;

  fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NDELAY); //FIXME: check that this worked
  fcntl(fd, F_SETFL, FNDELAY);
  options.c_cflag |= CRTSCTS;
  tcgetattr(fd, &options);
  cfsetispeed(&options, B57600);
  cfsetospeed(&options, B57600);
  options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
  options.c_cflag &= ~PARENB; /* Mask the character size to 8 bits, no parity */
  options.c_cflag &= ~CSTOPB;
  options.c_cflag &= ~CSIZE;
  options.c_cflag |= CS8; /* Select 8 data bits */
  options.c_cflag &= ~CRTSCTS; /* Disable hardware flow control */
  options.c_lflag &= ~(ICANON | ECHO | ISIG);
  tcsetattr(fd,TCSANOW, &options);
  return SUCCESS;
}

void mav_shutdown(void) {
  close(fd);
}



//returns the number parsed
unsigned int mav_messagesParse() {
  char buffer[1024];
  mavlink_message_t msg;
  mavlink_status_t status;
  mavlink_attitude_t *payload;
  unsigned int processed;
  int size;

  if(!fd)
    return 0;

  processed = 0;

  do {
    size = read(fd, buffer, sizeof(buffer));

    printf("read %d bytes\n", size);

    if(size < 0)
      return processed;

    for(unsigned int i = 0; i < size; i++) {
      if(mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, & status)) {
        processed++;
        // we have a complete mavlink packet, so go ahead and proccess it
        switch(msg.msgid) {
          case MAVLINK_MSG_ID_HEARTBEAT:
            printf("Recieved a heartbeat\n");
            break;
           // extend to process all other case we care about, if we don't care
           // then throw it into the default, (which is a nop)
	  case MAVLINK_MSG_ID_ATTITUDE:
	    payload = (mavlink_attitude_t *) &msg.payload64;
	    printf("Roll: %f Pitch: %f Yaw: %f Rollspeed: %f Pitchspeed: %f Yawspeed: %f\n", payload->roll, payload->pitch, payload->yaw, payload->rollspeed, payload->pitchspeed, payload->yawspeed);
            break;
          default: // a message we don't care about
            break;
        }

        //clear out the message before reading another one, this may be unnecesary
        memset(&msg, 0, sizeof(msg));
        memset(&status, 0, sizeof(status));
      }
    }
  } while (size < sizeof(buffer));

  return processed;
}

